# coding : utf-8

from matplotlib.lines import Line2D
from matplotlib.patches import Polygon
import numpy as np
from matplotlib import pyplot

from enum import Enum, unique

import dxfgrabber

import map_artist

from scipy.spatial import KDTree


@unique
class DirectionOfTrafficEnum(Enum):
    """
    道路通行方向
    """
    STRAIGHT = 0b0001
    TURN = 0b0010
    TREND = 0b0100
    BIDIRECTION = 0b1000


@unique
class RoadMaterial(Enum):
    UNDEFINED = 0
    PITCH = 1
    CEMENT = 2
    GRASS = 3


@unique
class AreaType(Enum):
    """
    道路附属物类型
    """
    UNDEFINED = 0
    TRAFFICLIGHT = 1
    CROSSAREA = 2
    VLOCALIZATIONSIGN = 3
    LLOCALIZATIONSIGN = 4
    WORKSHOP = 5
    STATICOBJECT = 6
    PARKING = 7  # 停车位
    BERTH = 8  # 泊位
    BRIDGE = 9  # 桥梁
    CLEANING = 10  # 清洗区
    CHARGING = 11  # 充电区
    YARD = 12  # 堆场
    ROAD = 13  # 普通道路


class MapLine(Line2D):
    def __init__(self, x=[], y=[], step=0.5, **kwargs):
        super().__init__(x, y, **kwargs)
        self.step = step
        self.boards = list([])
        self.marked_points = dict([])
        self.start = np.zeros((1, 2))
        self.end = np.zeros((1, 2))
        self.heading = np.zeros((1, 1))
        self.center = np.zeros((1, 2))
        self.start_angle = 0
        self.end_angle = 0
        self.radius = np.inf
        self.maptype = None
        self.objecttype = None
        self.configed = False
        self.selected = False
        # refline
        self.curvature = 0
        self.index = 0
        self.speed_limited = [0, 7]
        self.height_limited = 20
        self.lanes_number = 1
        self.ratio = 0.0  # 坡度
        self.direction = 0b0001
        self.slength = 0.0
        self.road_material = 0
        self.area = 0
        # refpoint
        self.width_list_ = list([])
        self.cuv_list_ = list([])
        self.gcuv_list_ = list([])
        self.slength_list_ = list([])
        # others
        self.ep_changed = False
        self.belongs = list([])

    def set_index(self, index):
        self.index = index
        self.set_url(index)

    def get_index(self):
        return self.index

    def setSelected(self):
        self.selected = True
        self.set_alpha(0.3)

    def setDeselected(self):
        self.selected = False
        self.set_alpha(1)

    def setDXFType(self, dxftype):
        """
        dxftype = 'line', 'arc', 'adjustment'
        """
        self.dxftype = dxftype

    def setMapType(self, maptype):
        """
        maptype = 'ref', 'board', 'appendix', etc
        """
        self.maptype = maptype

    def setDefaultAlpha(self):
        self.set_alpha(1)

    def setSelectedAlpha(self):
        self.set_alpha(0.3)

    def toggleAlpha(self):
        if self.selected:
            self.setSelectedAlpha()
        else:
            self.setDefaultAlpha()

    def equalDXFEntity(self, dxfentity):
        if dxfentity.dxftype != self.dxftype:
            return False
        else:
            if self.dxftype == 'LINE':
                l_ss = map_artist.calcPointsDistance(self.start, entity.start)
                l_ee = map_artist.calcPointsDistance(self.end, entity.end)
                l_se = map_artist.calcPointsDistance(self.start, entity.end)
                l_es = map_artist.calcPointsDistance(self.end, entity.start)
                if (l_ss == 0 and l_ee == 0) or (l_se == 0 and l_es == 0):
                    return True
                else:
                    return False
            elif self.dxftype == 'ARC':
                l_center = map_artist.calcPointsDistance(
                    self.center, entity.center)
                l_radius = self.radius - entity.radius
                l_ss_angle = self.start_angle - entity.start_angle * np.pi / 180
                l_ee_angle = self.end_angle - entity.end_angle * np.pi / 180
                l_se_angle = self.start_angle - entity.end_angle * np.pi / 180
                l_es_angle = self.end_angle - entity.start_angle * np.pi / 180
                if l_center == 0 and l_radius == 0 and (
                    (l_ss_angle == 0 and l_ee_angle == 0) or
                    (l_se_angle == 0 and l_es_angle == 0)):
                    return True
                else:
                    return False
            elif self.dxftype == 'POINT':
                l_pp = map_artist.calcPointsDistance(self.start, entity.point)
                if l_pp == 0:
                    return True
                else:
                    return False

    def fromDXFEntity(self, dxfentity, true_color):
        self.setDXFType(dxfentity.dxftype)
        self.set_color(true_color)
        self.true_color = true_color
        if self.dxftype == 'LINE':
            self.curvature = 0
            self.start, self.end = dxfentity.start, dxfentity.end
            self.slength = map_artist.calcPointsDistance(self.start, self.end)
            delta_x = (self.end[0] - self.start[0]) / self.slength * self.step
            delta_y = (self.end[1] - self.start[1]) / self.slength * self.step
            if delta_y == 0:
                xlist = np.arange(self.start[0], self.end[0], delta_x)
                ylist = np.ones(xlist.shape) * self.start[1]
            elif delta_x == 0:
                ylist = np.arange(self.start[1], self.end[1], delta_y)
                xlist = np.ones(ylist.shape) * self.start[0]
            else:
                xlist = np.arange(self.start[0], self.end[0], delta_x)
                ylist = np.arange(self.start[1], self.end[1], delta_y)
            xlist = np.append(xlist, self.end[0])
            ylist = np.append(ylist, self.end[1])
            self.set_xdata(xlist)
            self.set_ydata(ylist)
            angle = map_artist.calcAngle2Lines(np.array([self.start,
                                                         self.end]),
                                               np.array([
                                                   (0, 0), (0, 1)
                                               ]))  # l0 = line , l1 = y.axis
            self.heading = np.ones(self.get_xdata().shape) * angle
            self.slength_list_ = np.sqrt((xlist - xlist[0])**2 +
                                         (ylist - ylist[0])**2)
            self.cuv_list_ = np.zeros(self.slength_list_.shape)
            self.gcuv_list_ = np.zeros(self.slength_list_.shape)
        elif self.dxftype == 'ARC':
            self.curvature = 1 / dxfentity.radius
            self.start_angle = dxfentity.start_angle * np.pi / 180
            self.end_angle = dxfentity.end_angle * np.pi / 180
            self.radius = dxfentity.radius
            self.center = dxfentity.center
            if self.end_angle < self.start_angle:
                self.end_angle += np.pi * 2
            if self.end_angle > np.pi * 2:
                self.end_angle -= np.pi * 2
                self.start_angle -= np.pi * 2
            angle_step = self.step / self.radius
            angle_list = np.arange(self.start_angle, self.end_angle,
                                   angle_step)
            angle_list = np.append(angle_list, self.end_angle)
            self.set_xdata(self.radius * np.cos(angle_list) + self.center[0])
            self.set_ydata(self.radius * np.sin(angle_list) + self.center[1])
            self.start = np.array(self.get_xydata()[0])
            self.end = np.array(self.get_xydata()[-1])
            self.heading = -1 * angle_list  # dxf文件中,圆弧是逆时针,要转换为顺时针.同时,圆弧角度和切线角度的差异刚好对应北向角度,不需转换
            self.heading[self.heading > np.pi * 2] -= np.pi * 2
            self.heading[self.heading < 0] += np.pi * 2
            self.slength = (self.end_angle - self.start_angle) * self.radius
            self.slength_list_ = (angle_list - angle_list[0]) * self.radius
            self.cuv_list_ = np.ones(
                self.slength_list_.shape) * 1 / self.radius
            self.gcuv_list_ = np.zeros(self.slength_list_.shape)
        elif self.dxftype == 'POINT':
            self.curvature = 0
            self.start, self.end = dxfentity.point, dxfentity.point
            self.slength = 0
            self.set_xdata(self.start[0])
            self.set_ydata(self.start[1])

    def setMapParams(self, params):
        """
        set the parameters of the road
        """
        self.map_params = params

    def clearMapParams(self):
        """
        clear the params
        """
        self.map_params = None

    def addBoard(self, board):
        """
        add a board line
        """
        self.boards.append(board)

    def clearBoards(self):
        self.boards = list([])

    # def getDirection(self, startpoint):
    #     # todo
    #     ss_length = map_artist.calcPointsDistance(self._xy[0], startpoint)
    #     es_length = map_artist.calcPointsDistance(self._xy[-1], startpoint)
    #     if ss_length < es_length:
    #         self.direction = 0
    #     else:
    #         self.direction = -1
    #     return self.direction

    def pointToAppendixAttributeStr(self):
        if self.dxftype != 'POINT':
            return ''
        else:
            str_list = []

            str_list.append('{}'.format(self.index))  # index
            str_list.append('{}'.format(len(self.belongs)))  # belongs_number
            for i in self.belongs:
                str_list.append(str(i))
            str_list.append('{}'.format(self.objecttype))
            str_list.append('{}'.format(1))
            str_list.append('{}'.format(self.start[0]))
            str_list.append('{}'.format(self.start[1]))
            str_value = ','.join(str_list)
            return str_value

    def pointFromAppendixAttributeStr(self, str_value):
        value = str_value.split(',')
        belongs_number = int(value[1])
        self.objecttype = int(value[1 + belongs_number + 1])
        self.belongs = list(
            int(ref_id) for ref_id in value[2:2 + belongs_number])

    def makeRefPoints(self):
        # todo
        return

    def getHeadingValue(self):
        # todo
        # map_artist.calcAngle2Lines()
        return

    def addMarkedPoint(self, index, color, marker):
        if index in self.marked_points.keys():
            xy_data = self.get_xydata()
            marked_point = self.marked_points[index]
            marked_point.set_data([xy_data[index][0]], [xy_data[index][1]])
            marked_point.set_color(color)
            marked_point.set_marker(marker)
            return False
        else:
            xy_data = self.get_xydata()
            marked_point = Line2D([xy_data[index][0]], [xy_data[index][1]],
                                  color=color,
                                  marker=marker)
            # print(self._xy[index])
            self.marked_points[index] = marked_point
            return True

    def removeMarkedPoint(self, index):
        try:
            value = self.marked_points.pop(index)
            return value
        except KeyError:
            return None

    def setBelongs(self, belongs_id):
        self.belongs = belongs_id

    def setObjectType(self, objecttype):
        self.objecttype = objecttype
        self.set_color('black')

    def clearObjectType(self):
        self.objecttype = None
        self.set_color(self.true_color)

    def setConfiged(self):
        self.configed = True
        self.set_color('black')

    def setReconfig(self):
        self.configed = False
        self.set_color(self.true_color)

    def copyRefLine(self, ref_line):
        self.index = ref_line.index
        self.speed_limited = ref_line.speed_limited
        self.height_limited = ref_line.height_limited
        self.curvature = ref_line.curvature
        self.lanes_number = ref_line.lanes_number
        self.ratio = ref_line.ratio
        self.direction = ref_line.direction
        self.slength = ref_line.slength
        self.start = ref_line.start
        self.end = ref_line.end
        self.road_material = ref_line.road_material
        self.area = ref_line.area

    def toRefLineStr(self):
        # write title
        # file.write('index, speed_low, speed_up, lanes, ratio, direction, slength, startpoint, endpoint, material\n')
        style_list = []
        style_list.append('{}'.format(self.get_index()))
        style_list.append('{}'.format(self.speed_limited[0]))
        style_list.append('{}'.format(self.speed_limited[1]))
        style_list.append('{}'.format(self.height_limited))
        style_list.append('{}'.format(self.curvature))
        style_list.append('{}'.format(int(self.lanes_number)))
        style_list.append('{}'.format(-1 * float(self.ratio)))
        style_list.append('{}'.format(int(self.direction)))
        style_list.append('{}'.format(self.slength))
        style_list.append('{}'.format(self.start[0]))
        style_list.append('{}'.format(self.start[1]))
        style_list.append('{}'.format(0.0))  # (self.startpoint[2]))
        style_list.append('{}'.format(self.end[0]))
        style_list.append('{}'.format(self.end[1]))
        style_list.append('{}'.format(0.0))  # (self.endpoint[2]))
        style_list.append('{}'.format(int(self.road_material)))
        style_list.append('{}'.format(int(self.area)))
        style_str = ','.join(style_list)
        return style_str

    def toNegRefLineStr(self):
        style_list = []
        style_list.append('{}'.format(-1 * self.get_index()))
        style_list.append('{}'.format(self.speed_limited[0]))
        style_list.append('{}'.format(self.speed_limited[1]))
        style_list.append('{}'.format(self.height_limited))
        style_list.append('{}'.format(self.curvature))
        style_list.append('{}'.format(int(self.lanes_number)))
        style_list.append('{}'.format(-1 * float(self.ratio)))
        style_list.append('{}'.format(int(self.direction)))
        style_list.append('{}'.format(self.slength))
        style_list.append('{}'.format(self.end[0]))
        style_list.append('{}'.format(self.end[1]))
        style_list.append('{}'.format(0.0))  # (self.startpoint[2]))
        style_list.append('{}'.format(self.start[0]))
        style_list.append('{}'.format(self.start[1]))
        style_list.append('{}'.format(0.0))  # (self.endpoint[2]))
        style_list.append('{}'.format(int(self.road_material)))
        style_list.append('{}'.format(int(self.area)))
        style_str = ','.join(style_list)
        return style_str

    def fromRefLineStr(self, style_str):
        str_list = style_str.split(',')
        # self.index = int(str_list[0])
        self.speed_limited[0] = float(str_list[1])
        self.speed_limited[1] = float(str_list[2])
        self.height_limited = float(str_list[3])
        # self.curvature = float(str_list[4])
        self.lanes_number = int(str_list[5])
        self.ratio = float(str_list[6])
        self.direction = int(str_list[7])
        # self.slength = float(str_list[8])
        start = np.array([float(str_list[9]),
                          float(str_list[10])])  #,float(str_list[11] = 0))
        end = np.array([float(str_list[12]),
                        float(str_list[13])])  #,float(str_list[14] = 0))
        self.road_material = int(str_list[15])
        self.area = int(str_list[16])
        return start, end

    def toRefPointStr(self, index, sep=','):
        xdata = self.get_xdata()
        ydata = self.get_ydata()
        style_list = []
        style_list.append('{}'.format(self.index))
        style_list.append('{}'.format(xdata[index]))
        style_list.append('{}'.format(ydata[index]))
        style_list.append('{}'.format(0.0))
        style_list.append('{}'.format(self.lanes_number))
        for width in self.width_list_[index]:
            style_list.append('{}'.format(width[0]))
            style_list.append('{}'.format(width[1]))
            # # for test
            # if width[0]<0 or width[1] < 0:
            #     print('elememtn: got a negative', style_list)
        style_list.append('{}'.format(self.cuv_list_[index]))
        style_list.append('{}'.format(self.gcuv_list_[index]))
        style_list.append('{}'.format(self.slength_list_[index]))
        style_list.append('{}'.format(self.heading[index]))
        style_str = sep.join(style_list)
        return style_str

    def toNegRefPointStr(self, index, sep=','):
        xdata = self.get_xdata()
        ydata = self.get_ydata()
        length = len(xdata)
        style_list = []
        style_list.append('{}'.format(-1 * self.index))
        style_list.append('{}'.format(xdata[length - index - 1]))
        style_list.append('{}'.format(ydata[length - index - 1]))
        style_list.append('{}'.format(0.0))
        style_list.append('{}'.format(self.lanes_number))
        for width in reversed(self.width_list_[length - index - 1]):
            style_list.append('{}'.format(-1 * width[1]))
            style_list.append('{}'.format(-1 * width[0]))
            # # for test
            # if width[0]<0 or width[1] < 0:
            #     print('elememtn: got a negative', style_list)
        style_list.append('{}'.format(-1 * self.cuv_list_[length - index - 1]))
        style_list.append('{}'.format(-1 *
                                      self.gcuv_list_[length - index - 1]))
        style_list.append('{}'.format(self.slength_list_[-1] -
                                      self.slength_list_[length - index - 1]))
        heading = self.heading[length - index - 1] + np.pi
        if heading > 2 * np.pi:
            heading -= 2 * np.pi
        style_list.append('{}'.format(heading))
        style_str = sep.join(style_list)
        return style_str

    def fromRefPointStr(self, style_str, sep=','):
        """
        FIXME
        """
        style_list = style_str.split(sep)
        self._x.extend([float(style_list[0])])
        self._y.extend([float(style_list[1])])
        # self.gps_[2] = float(style_list[2])
        index = 3
        self.lanes_number = int(style_list[3])

        widthes = []
        for _ in range(self.lanes_number):
            width = [0, 0]
            index += 1
            width[0] = float(style_list[index])
            index += 1
            width[1] = float(style_list[index])
            widthes.append(width)
        self.width_list_.append(widthes)
        index += 1
        self.cuv_list_.append(float(style_list[index]))
        index += 1
        self.gcuv_list_.append(float(style_list[index]))
        index += 1
        self.slength_list_.append(float(style_list[index]))
        index += 1
        self.heading.append(float(style_list[index]))

    def changeEndPoints(self):
        """
        因为ep更改,所以相关的序列都需要更改
        1.start\end
        2.start_angle\end_angle
        3._slength
        4._heading
        5._dappar
        6._kappar
        7._x
        8._y
        """
        xy_data = self.get_xydata()
        xy_data = np.flip(xy_data, 0)
        self.set_data(xy_data[:, 0], xy_data[:, 1])
        self.start, self.end = self.end, self.start
        self.start_angle, self.end_angle = self.end_angle, self.start_angle
        self.heading = self.heading[::-1] + np.pi  # 逆序,并将朝向旋转180度
        self.heading[self.heading > np.pi * 2] -= np.pi * 2  # 0~2pi
        self.slength_list_ = self.slength - self.slength_list_[::
                                                               -1]  # 逆序,并减掉累计量
        self.cuv_list_ = self.cuv_list_[::-1] * -1  # 逆序,并交换减法元素
        self.gcuv_list_ = self.gcuv_list_[::-1] * -1
        return

    def calcWidth(self):
        """
        boards_xydata: 与self.lanes_number相关的车道边界数据
        """
        xy_data = self.get_xydata()
        for point in xy_data:
            distance_list = list([])
            for board in self.boards_xydata:
                board_tree = KDTree(board)
                distance, _ = board_tree.query(point, 1)
                distance_list.append(distance)
            widthes = []
            for i in range(self.lanes_number):
                width = np.array(
                    distance_list[i:i + 2]) * self.width_sign[i:i + 2]
                widthes.append(width)
            self.width_list_.append(widthes)
        return

    def sortBoards(self, boards_xydata):
        """
        对board进行重排序，保证按照由左到右（按照行车方向）排列
        1.计算board中与start最近点
        2.计算该最近点与行车方向的夹角，当为正时，在左侧；反之在右侧
        输入: boards_xydata: 边界数据(每条边界为一个list,可能包含条线,多条边界组成一个list)
        """
        self.boards_xydata = []
        xy_data = self.get_xydata()
        distance_list = list([])
        for data in boards_xydata:
            board_tree = KDTree(data)
            distance, index = board_tree.query(xy_data[0], 1)
            board_point = data[index]
            angle = map_artist.calcAngle2Lines((xy_data[0], xy_data[1]),
                                               (xy_data[0], board_point))
            if angle < np.pi:
                angle = -1
            else:
                angle = 1
            distance *= angle
            distance_list.append(distance)

        index_sort = np.argsort(distance_list)
        self.width_sign = np.sort(distance_list)
        self.width_sign[self.width_sign < 0] = -1
        self.width_sign[self.width_sign >= 0] = 1
        self.width_sign = np.array(self.width_sign)
        for i in index_sort:
            self.boards_xydata.append(boards_xydata[i])

    def saveRefLine(self, file_name):

        return

    def saveRefPoint(self, file_name):
        return


if __name__ == "__main__":
    from map_artist.map_polygon import MapPolygon

    def onPick(event):
        print('pick a line')

    def picker(aritist, mousevent):
        print('picker function called')

    def polygonPickHandle(artist, mouseevent):
        print('pick a polygon')

    figure = pyplot.figure(1)
    figure.add_subplot(111)
    ax = figure.gca()

    file = dxfgrabber.readfile('/home/ads/my_code/map_tools/data/demo.dxf')
    for entity in file.entities:
        a_map_line = MapLine(step=0.5)
        true_color = map_artist.getEntityTrueColor(file.layers, entity)
        print('true_color = {}'.format(true_color))
        a_map_line.fromDXFEntity(entity, true_color)
        a_map_line.set_picker(True)
        ax.add_line(a_map_line)

    point = a_map_line.removeMarkedPoint(0)
    if point is not None:
        ax.lines.remove(point)
    else:
        print('no marked point now, add one')
        a_marked_point = a_map_line.addMarkedPoint(0, 'r', 'o')
        ax.add_line(a_marked_point)
    figure.canvas.mpl_connect('pick_event', onPick)
    ax.margins(0)

    x = np.arange(0, 10, 0.5)
    print(x.shape)
    y = x**2
    data = np.vstack((x, y))
    data = np.transpose(data)
    print(data.shape)
    a_map_polygon = MapPolygon(data, closed=False)
    a_map_polygon.set_picker(10)
    ax.add_patch(a_map_polygon)

    pyplot.show()
